#include <iostream>
#include <fstream>
#include <chrono>
#include <algorithm>
#include <thread>
#include "dji_flight_handler.h"
#include "assist_land.h"
#include "assist_avoid.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace std;
using namespace cv;

void asssitLandHandler();
void startMopCommunicationServer();
void assistAvoidHandler();

int main(int argc, char **argv)
{
	Application application(argc, argv);

	bool ret;

	//start flightHandler for obtaining information from dji flight
	ret = flightHandler.startUpSystem();

	if(!ret)
	{
		cout << "start up flightHandler success" << endl;
		return -1;
	}

	//start a thread for handle the mop channel message.
	std::thread mopCommunicationWithMsdkThread(startMopCommunicationServer);

	//start assist land mode
	std::thread assistLandThread(asssitLandHandler);

	//start assist avoid mode
	std::thread assistAvoidThread(assistAvoidHandler);

	bool statusLedFlag;

	while(1)
	{
		statusLedFlag = !statusLedFlag;
		if(statusLedFlag)
		{
			system("echo 0 > /sys/class/gpio/gpio21/value");
		}
		else
		{
			system("echo 1 > /sys/class/gpio/gpio21/value");
		}

		flightHandler.sleepMs(1000);
		flightHandler.sendHeartBeatToMsdk();
		flightHandler.noHeartCountSelfAdd();

		//write the cpu temperature to log
		flightHandler.recodeCpuTemperature();
	}

	mopCommunicationWithMsdkThread.join();
	assistLandThread.join();
	assistAvoidThread.join();
	return 0;
}

void asssitLandHandler()
{
	assistLand.init(&flightHandler);
	assistLand.run();
}

void startMopCommunicationServer()
{
	flightHandler.mopCommunicationWithMsdkRun();
}

void assistAvoidHandler()
{
	assistAvoid.init(&flightHandler);
	assistAvoid.run();
}